/**
 * @file		init.s
 * @brief		Initialize Procedure
 * @note		None
 * @attention	None
 * 
 * <B><I>Copyright 2015 Socionext Inc.</I></B>
 */
#define CO_ASM_MODE
#include "common_memory.h"

//---------------------------------------------------------------
//   Condition Assenble Control
//---------------------------------------------------------------
	.text
	.section .INIT_CODE
    .align 2

//---------------------------------------------------------------
// Reset Entry
//---------------------------------------------------------------
	.type	Init_CODE, %function
	.global	Init_CODE
_Reset_Handler:
Init_CODE:

//---------------------------------------------------------------
//   Include Files(s)
//---------------------------------------------------------------
    .extern init_gic
    .extern init_dist_bank_gic
    .extern init_cpu_bank_gic
    .extern enable_gic
    .extern init_gpv
	.extern	boot
	.extern	gTkernel_jump_addr
	.extern	gLinux_jump_addr

	.global	_Reset_Handler
	.global  __linux_jump
	.global  __led13_on
	.global  __led13_off
	.global  __led12_on
	.global  __led12_off
	.global  __led11_on
	.global  __led11_off

//---------------------------------------------------------------
//   Define(s)
//---------------------------------------------------------------
	.equ	CPU0_NUM,			0
	.equ	CPU1_NUM,			1
	.equ	CPU2_NUM,			2
	.equ	CPU3_NUM,			3
	.equ	TIMESTAMP_BASE,		0x1D010000
	.equ	TIMESTAMP_FID0,		0x02625A00
	.equ	GICD_CTLR,			0x1D001000
	.equ	WAIT_ADR,			__common_start
	.equ	WAIT_VAL,			0xD1C3B5A7
	.equ	FIQ_MODE,			0x11
	.equ	IRQ_MODE,			0x12
	.equ	SVC_MODE,			0x13
	.equ	ABT_MODE,			0x17
	.equ	UND_MODE,			0x1B
	.equ	SYS_MODE,			0x1F
	.equ	PDR,				0x1DFFB000
	.equ	DDR,				0x1DFFB100
	.equ	EPCR,				0x1DFFB200
#ifdef CO_RTOS_CPUx2
	.equ	END_LOAD_LINUX,		SDRAM_ADR_LINUX_RESUME_MEMORY_SECOND_CPU
	.equ	KERNEL_UNBOOT_FLAG,		0x12345678
	.equ	TRAPOLINE_ADDRESS,		END_LOAD_LINUX
	.equ	DRAM_MULTI_CORE_CODE_AREA,	(SDRAM_ADR_LINUX_COMPLITE_SUSPEND + 4*4)
#endif

//---------------------------------------------------------------
// Macro(s)
//---------------------------------------------------------------
#ifdef CO_MEASURE_BOOTUP_TIME
	.macro led11_on
	// set GPIO P25
	LDR		r1, =EPCR
	LDR		r0, [r1, #0x4]
	BIC		r0, r0, #0x20
	STR		r0, [r1, #0x4]
	// set Direction OUT
	LDR		r1, =DDR
	LDR		r0, [r1, #0x4]
	ORR		r0, r0, #0x20
	STR		r0, [r1, #0x4]
	// set High
	LDR		r1, =PDR
	LDR		r0, =0x00200020
	STR		r0, [r1, #0x4]
	.endm

	.macro led11_off
	// set Low
	LDR		r1, =PDR
	LDR		r0, =0x00200000
	STR		r0, [r1, #0x4]
	.endm

	.macro led12_on
	// set GPIO P26
	LDR		r1, =EPCR
	LDR		r0, [r1, #0x4]
	BIC		r0, r0, #0x40
	STR		r0, [r1, #0x4]
	// set Direction OUT
	LDR		r1, =DDR
	LDR		r0, [r1, #0x4]
	ORR		r0, r0, #0x40
	STR		r0, [r1, #0x4]
	// set High
	LDR		r1, =PDR
	LDR		r0, =0x00400040
	STR		r0, [r1, #0x4]
	.endm

	.macro led12_off
	// set Low
	LDR		r1, =PDR
	LDR		r0, =0x00400000
	STR		r0, [r1, #0x4]
	.endm

	.macro led13_on
	// set GPIO P27
	LDR		r1, =EPCR
	LDR		r0, [r1, #0x4]
	BIC		r0, r0, #0x80
	STR		r0, [r1, #0x4]
	// set Direction OUT
	LDR		r1, =DDR
	LDR		r0, [r1, #0x4]
	ORR		r0, r0, #0x80
	STR		r0, [r1, #0x4]
	// set High
	LDR		r1, =PDR
	LDR		r0, =0x00800080
	STR		r0, [r1, #0x4]
	.endm

	.macro led13_off
	// set Low
	LDR		r1, =PDR
	LDR		r0, =0x00800000
	STR		r0, [r1, #0x4]
	.endm
#endif

	// get_cpuid
	.macro get_cpuid  reg
	MRC		p15, 0, \reg, c0, c0, 5		// Read CP15 Main ID Register (MIDR) into \reg
	AND		\reg, \reg, #0xf
	.endm

	// disable_mem
	.macro disable_mem  reg
	MRC		p15, 0, \reg, c1, c0, 0		// Read CP15 System Control Register (SCTLR) into \reg
	BIC		\reg, \reg, #(1 << 0)		// Clear MMU enable
	BIC		\reg, \reg, #(1 << 2)		// Disable D-cache
	BIC		\reg, \reg, #(1 << 11)      // Branch Prediction Disable
	BIC		\reg, \reg, #(1 << 12)		// Disable I-cache
	MCR		p15, 0, \reg, c1, c0, 0		// Write value back
	.endm

//---------------------------------------------------------------
// Reset Entry
//---------------------------------------------------------------
#ifdef CO_MEASURE_BOOTUP_TIME
	led12_on
#endif

	// MMU disable
	// All data accesses are treated as Non-cacheable and Strongly-ordered
	// All instruction accesses are Non-cacheable, because SCTLR. I-bit = 0 for Cortex-A5.
	disable_mem r0

	MRC		p15, 0, r0, c1, c0, 1				// Read CP15 Auxiliary Control Register (ACTLR)
	BIC		r0, r0, #(3 << 13)					// Data prefetching disable
	ORR		r0, #(1 << 6)						// SMP Mode
	MCR		p15, 0, r0, c1, c0, 1				// Write value back
	DSB
	ISB

	// You must invalidate the instruction cache, the data cache, TLB, and BTAC before using them.[TRM]
	// Invalidate L1 Caches
	MOV		r0, #0
	MCR		p15, 0, r0, c8, c7, 0		// TLBIALL - Invalidate entire Unifed TLB
	MCR		p15, 0, r0, c7, c5, 6		// BPIALL  - Invalidate entire branch predictor array
	MCR		p15, 0, r0, c7, c5, 0		// ICIALLU - Invalidate all instruction cache to PoU

//---------------------------------------------------------------
	get_cpuid r1
	CMP		r1, #CPU0_NUM
	BEQ		__timestamp_set
	BNE		__timestamp_end

__timestamp_set:
	LDR		r0, = TIMESTAMP_BASE
	LDR		r1, = TIMESTAMP_FID0
	LDR		r2, = 0x0
	LDR		r3, = 0x1
	STR		r2, [r0, #0]				// CNTCR
	STR		r2, [r0, #0x8]				// CNTCVL
	STR		r2, [r0, #0xC]				// CNTCVU
	STR		r1, [r0, #0x20]				// CNTFID0
	STR		r3, [r0, #0]				// CNTCR

__timestamp_end:

//---------------------------------------------------------------
	get_cpuid r1
	CMP		r1, #CPU0_NUM
	BEQ		__stack_cpu0_set
	CMP	    r1, #CPU1_NUM
	BEQ		__stack_cpu1_set
	CMP	    r1, #CPU2_NUM
	BEQ		__stack_cpu2_set
	CMP	    r1, #CPU3_NUM
	BEQ		__stack_cpu3_set

__stack_cpu0_set:
	//CPSID	aif, #SVC_MODE		// SVC Mode
	LDR		sp, =STACK_AREA_SVC_CPU0
	B		__stack_end
__stack_cpu1_set:
	//CPSID	aif, #SVC_MODE		// SVC Mode
	LDR		sp, =STACK_AREA_SVC_CPU1
	B		__stack_end
__stack_cpu2_set:
	//CPSID	aif, #SVC_MODE		// SVC Mode
	LDR		sp, =STACK_AREA_SVC_CPU2
	B		__stack_end
__stack_cpu3_set:
	//CPSID	aif, #SVC_MODE		// SVC Mode
	LDR		sp, =STACK_AREA_SVC_CPU3
	B		__stack_end

__stack_end:

//---------------------------------------------------------------
    LDR		r0, =RAM_AREA
	MCR 	p15, 0, r0, c12, c0, 0 // Write CP15 Vector Base Address Register (secure)
	ISB
//---------------------------------------------------------------
	get_cpuid r1
	CMP		r1, #CPU0_NUM
	BEQ		__gic_set
	BNE		__gic_bank_set

__gic_set:
	BL		init_gic
	BL		init_dist_bank_gic
	BL		init_cpu_bank_gic
	BL		enable_gic
	B		__gic_end

__gic_bank_set:
	LDR		r0, = GICD_CTLR
	LDR		r1, [r0]
#if 1
	CMP		r1, #0x00000003
#else
	CMP		r1, #0x00000001
#endif

	BNE		__gic_bank_set
	BL		init_dist_bank_gic
	BL		init_cpu_bank_gic

__gic_end:

//---------------------------------------------------------------
	get_cpuid r1
	CMP		r1, #CPU0_NUM
	BEQ		__rtos_jump
	CMP	    r1, #CPU1_NUM
	BEQ		__rtos_jump
	CMP	    r1, #CPU2_NUM
#ifndef CO_RTOS_CPUx2
	BEQ		__rtos_jump
#else
	BEQ		__linux_jump2
#endif
	// cpu3
	BL		cpu3_spi_set

//---------------------------------------------------------------
    LDR		r0, =__common_start
    LDR		r1, =__common_end
    SUB		r1, r1, r0
    LDR		r2, = 0x00000000
linux_tkernel_common_area_init:
	STR		r2, [r0], #4
	SUBS	r1, #4
	BNE		linux_tkernel_common_area_init

// Copy data
	LDR	r0, =__data_org
	LDR	r2, =__data_start
	LDR	r4, =__data_size
	CMP	r4, #0
	BEQ	2f
1:
	LDMIA   r0!, {r5-r12}
	STMIA   r2!, {r5-r12}
	SUBS    r4, #32
	BNE     1b
2:

	/* bss */
	ldr     r1, =__bss_start
	ldr     r2, =__bss_end
	cmp     r1, r2
	bhs     2f
	mov     r0, #0
1:
	str     r0, [r1], #4
	cmp     r1, r2
	blo     1b
2:
	ldr     r12, =boot
	blx     r12
3:
	b       3b

//---------------------------------------------------------------
__rtos_jump:
	LDR		r1, = WAIT_ADR
	MOV		r0, #0x0
	STR		r0, [r1]

	DSB
	WFE

	LDR		r1, = WAIT_ADR
	LDR		r2, = WAIT_VAL

__rtos_jump_loop:
    LDR		r0, [r1]
    CMP		r0, r2
    BNE		__rtos_jump_loop
	DMB

//---------------------------------------------------------------
#ifdef CO_LINUX_VERSION_ON
#ifndef CO_SUB_CPU_JUMP_ON
	B		__rtos_jump_end
#endif
#endif
	BL		init_gpv
	LDR		r0, = gTkernel_jump_addr
	LDR		r1, [r0]
	CMP		r1, #0x0
	BXNE	r1
__rtos_jump_end:
	WFE
	B		__rtos_jump_end

//---------------------------------------------------------------
__linux_jump:
#ifdef CO_LINUX_VERSION_ON
	LDR		r1, = WAIT_ADR
	LDR		r0, = WAIT_VAL
	STR		r0, [r1]

	SEV
#endif

//---------------------------------------------------------------
#ifdef CO_MEASURE_BOOTUP_TIME
	led12_off
#endif

#ifndef CO_LINUX_VERSION_ON
#ifndef CO_SUB_CPU_JUMP_ON
	B		__linux_jump_end
#endif
#endif
	LDR		r0, = gLinux_jump_addr
	LDR		r1, [r0]
	CMP		r1, #0x0
#ifndef CO_RTOS_CPUx2
	BXNE	r1
__linux_jump_end:
	WFE
	B		__linux_jump_end
#else
	BEQ		__linux_jump
	LDR		r0, = END_LOAD_LINUX
	MOV		r2, #0x1
	STR		r2, [r0]

	LDR		r3, =DRAM_MULTI_CORE_CODE_AREA
	ADR		r2, start_wait
	ADR		r5, end_wait

copy_secondry_code:
	LDR		r0, [r2], #4
	STR		r0, [r3], #4
	CMP		r2, r5
	BNE		copy_secondry_code
	DSB

	LDR		r0, =TRAPOLINE_ADDRESS
	LDR		r2, =KERNEL_UNBOOT_FLAG
	STR		r2, [r0, #4]
	STR		r2, [r0, #8]
	STR		r2, [r0, #12]
	STR		r2, [r0]
	BX		r1

start_wait:
	/* wait for Primay/CPU0 to have gone past */
pri_catchup:
	LDR		r3, [r0]
	CMP		r3, r2
	BNE		pri_catchup

	WFE
wwfi1:
	LDR		r3, [r0, r1, lsl #2]
	CMP		r3, r2
	BEQ		wwfi1
	DMB
	BX		r3
end_wait:

__linux_jump_end:
	B		__linux_jump

__linux_jump2:
	LDR		r0, = END_LOAD_LINUX
	MOV		r1, #0x0
	STR		r1, [r0]
	WFE

__linux_wait_load:
	LDR		r1, [r0]
	CMP		r1, #0x0
	BEQ		__linux_wait_load
	LDR		r0, = gLinux_jump_addr
	LDR		r1, [r0]

	LDR		r0, =TRAPOLINE_ADDRESS
	LDR		r2, =KERNEL_UNBOOT_FLAG
1:
	LDR		r1, [r0]
	CMP		r1, r2
	BNE		1b

	get_cpuid r1
	LDR		r3, =DRAM_MULTI_CORE_CODE_AREA
	BX		r3

#endif


//---------------------------------------------------------------
__led13_on:
#ifdef CO_MEASURE_BOOTUP_TIME
	led13_on
#endif
	BX		lr

__led13_off:
#ifdef CO_MEASURE_BOOTUP_TIME
	led13_off
#endif
	BX		lr
	
__led12_on:
#ifdef CO_MEASURE_BOOTUP_TIME
	led12_on
#endif
	BX		lr

__led12_off:
#ifdef CO_MEASURE_BOOTUP_TIME
	led12_off
#endif
	BX		lr

__led11_on:
#ifdef CO_MEASURE_BOOTUP_TIME
	led11_on
#endif
	BX		lr

__led11_off:
#ifdef CO_MEASURE_BOOTUP_TIME
	led11_off
#endif
	BX		lr

    .end
